#include "utils.hpp"
#include <chrono>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h> // TicToc
#include <pcl/console/print.h>

#include <pcl/registration/ndt.h>    //NDT(正态分布)配准类头文件
#include <pcl/registration/icp.h>    //ICP类相关头文件
#include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件
#include <pcl/registration/gicp.h>   //Generalized ICP 相关头文件
// 特征
#include <pcl/features/normal_3d.h>             //法向量特征
#include <pcl/features/normal_3d_omp.h>         //法向量特征
#include <pcl/filters/approximate_voxel_grid.h> //滤波类头文件  （使用体素网格过滤器处理的效果比较好）

#include <fast_gicp/gicp/fast_gicp.hpp>
#include <fast_gicp/gicp/fast_gicp_st.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>
#ifdef USE_VGICP_CUDA
#include <fast_gicp/ndt/ndt_cuda.hpp>
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#endif

using PointType = pcl::PointXYZ;
using PointNormalType = pcl::PointNormal;
bool nextIteration = true;
/**
 * 此函数是查看器的回调。 当查看器窗口位于顶部时，只要按任意键，就会调用此函数。 如果碰到“空格”； 将布尔值设置为true。
 * @param event
 * @param nothing
 */
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event,
                           void *nothing)
{
    if (event.getKeySym() == "space" && event.keyDown())
        nextIteration = true;
}
////////////////////////////////////////////////////////////////////////////////
/** \brief Align a pair of PointCloud datasets and return the result
 * \param aligner the alignment algorithm instance
 * \param cloudSrc the source PointCloud
 * \param cloudTgt the target PointCloud
 * \return the resultant transform between source and target and the resultant aligned source PointCloud
 */
/// 实现匹配，其中参数有输入一组需要配准的点云，0其他参数输出配准后的点云以及变换矩阵
template <typename PointT>
void PairAlign(pcl::Registration<PointT, PointT> &aligner, const typename pcl::PointCloud<PointT>::Ptr &cloudSrc, const typename pcl::PointCloud<PointT>::Ptr &cloudTgt, Eigen::Matrix4f &transform, int maxIteration = 1)
{

    // 设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
    // 添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
    aligner.setMaximumIterations(maxIteration);

    auto t1 = std::chrono::high_resolution_clock::now();
    aligner.setInputSource(cloudSrc); // 源点云
    // Setting point cloud to be aligned to.
    aligner.setInputTarget(cloudTgt); // 目标点云
    // 计算需要的刚体变换以便将输入的源点云匹配到目标点云
    typename pcl::PointCloud<PointT>::Ptr alignedCloud(new pcl::PointCloud<PointT>);
    aligner.align(*alignedCloud, transform);
    auto t2 = std::chrono::high_resolution_clock::now();
    double costTime = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    // 这个地方的alignedCloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
    std::cout << "Applied " << aligner.getMaximumIterations() << " iteration(s) in " << costTime << " ms" << std::endl;
    std::cout << "Fitness score: " << aligner.getFitnessScore() << std::endl;
    transform = aligner.getFinalTransformation();
}
bool downsample = true;
double leafsize = 1.0;
int main(int argc, char **argv)
{
    pcl::NormalEstimation<PointType, PointNormalType> normEst; // 点云法线估计对象

    pcl::Registration<PointNormalType, PointNormalType>::Ptr reg;
    pcl::IterativeClosestPoint<PointNormalType, PointNormalType>::Ptr icp(new pcl::IterativeClosestPoint<PointNormalType, PointNormalType>);
    pcl::IterativeClosestPointNonLinear<PointNormalType, PointNormalType>::Ptr icp_nl(new pcl::IterativeClosestPointNonLinear<PointNormalType, PointNormalType>);
    pcl::GeneralizedIterativeClosestPoint<PointNormalType, PointNormalType>::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint<PointNormalType, PointNormalType>);
    pcl::IterativeClosestPointWithNormals<PointNormalType, PointNormalType>::Ptr icp_normals(new pcl::IterativeClosestPointWithNormals<PointNormalType, PointNormalType>);
    pcl::NormalDistributionsTransform<PointNormalType, PointNormalType>::Ptr pcl_ndt(new pcl::NormalDistributionsTransform<PointNormalType, PointNormalType>);
    pcl_ndt->setStepSize(0.1);   // 为more-thuente线搜索设置最大步长
    pcl_ndt->setResolution(1.0); // 设置NDT网格网格结构的分辨率（voxelgridcovariance）
    // icp_normals->setUseSymmetricObjective(true);
    fast_gicp::FastGICPSingleThread<pcl::PointXYZ, pcl::PointXYZ> fgicp_st;
    fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> fgicp_mt; // fast_gicp uses all the CPU cores by default

    fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> vgicp;
    vgicp.setResolution(1.0);
    vgicp.setNumThreads(omp_get_max_threads());

    if (argc < 4)
    {
        pcl::console::print_highlight("Usage: %s path/to/pcd/dir algorithm max_iteration\n", argv[0]);
        pcl::console::print_highlight("algorithm: 0 for pcl::IterativeClosestPoint\n");
        pcl::console::print_highlight("           1 for pcl::IterativeClosestPointNonLinear\n");
        pcl::console::print_highlight("           2 for pcl::GeneralizedIterativeClosestPoint\n");
        pcl::console::print_highlight("           3 for pcl::IterativeClosestPointWithNormals\n");
        pcl::console::print_highlight("           4 for pcl::NormalDistributionsTransform\n");
        return -1;
    }
    int algo = std::stoi(argv[2]);
    switch (algo)
    {
    case 0:
        reg = icp;
        break;
    case 1:
        reg = icp_nl;
        break;
    case 2:
        reg = gicp;
        break;
    case 3:
        reg = icp_normals;
        break;
    case 4:
        reg = pcl_ndt;
        break;
    default:
        break;
    }
    int maxIt = std::stoi(argv[3]);
    std::vector<std::string> filenames = GetFilesInDir(argv[1], ".pcd");
    if (filenames.empty())
    {
        std::cout << "There is no pcd file in pcd directory!" << std::endl;
        return -1;
    }

    // 加载所有点云文件
    std::vector<pcl::PointCloud<PointNormalType>::Ptr> cloudsIn;
    std::vector<pcl::PointCloud<PointNormalType>::Ptr> cloudsRes;
    std::unique_ptr<pcl::VoxelGrid<PointType>> grid; // 滤波处理对象
    // Downsample for consistency and speed
    // \note enable this for large datasets
    if (downsample)
    {
        grid = std::make_unique<pcl::VoxelGrid<PointType>>();
        grid->setLeafSize(leafsize, leafsize, leafsize); // 设置滤波时采用的体素大小
    }

    pcl::PCDReader pcdReader;
    for (const auto &filename : filenames)
    {
        pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
        pcl::PointCloud<PointType>::Ptr downCloud(new pcl::PointCloud<PointType>);
        pcl::PointCloud<PointNormalType>::Ptr normalCloud(new pcl::PointCloud<PointNormalType>);
        if (pcdReader.read(filename, *cloud) != 0)
        {
            pcl::console::print_highlight("Couldn't read file: %s\n", filename);
            return (-1);
        }
        normEst.setKSearch(50);
        // normEst.setRadiusSearch(0.5);
        if (bool(grid))
        {
            grid->setInputCloud(cloud);
            grid->filter(*downCloud);
            normEst.setInputCloud(downCloud);
            normEst.setSearchSurface(cloud);
        }
        else
        {
            normEst.setInputCloud(cloud);
        }

        pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>());
        normEst.setSearchMethod(tree);
        normEst.compute(*normalCloud);
        if (bool(grid))
        {
            pcl::copyPointCloud(*downCloud, *normalCloud);
        }
        else
        {
            pcl::copyPointCloud(*cloud, *normalCloud);
        }
        cloudsIn.emplace_back(normalCloud);
    }
    cloudsRes = cloudsIn;
    std::vector<Eigen::Matrix4f> absTfs(cloudsIn.size(), Eigen::Matrix4f::Identity());
    std::vector<Eigen::Matrix4f> pairTfs(cloudsIn.size() - 1, Eigen::Matrix4f::Identity());
    // 初始化点云可视化对象
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    int v1(0);                                          // 创建新的视口
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);     // 4个参数分别是X轴的最小值，最大值，Y轴的最小值，最大值，取值0-1，v1是标识
    viewer->setBackgroundColor(0, 0, 0, v1);            // 设置视口的背景颜色
    viewer->addText("original", 10, 10, "v1 text", v1); // 添加一个标签区别其他窗口  利用RGB颜色着色器并添加点云到视口中
    viewer->addCoordinateSystem(1.0, "viewport1", v1);  // 显示XYZ指示轴
    // 对第二视口做同样的操作，使得做创建的点云分布于右半窗口
    int v2(0);
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
    viewer->addText("aligned", 10, 10, "v2 text", v2);
    viewer->addCoordinateSystem(1.0, "viewport2", v2); // 显示XYZ指示轴
    // Register keyboard callback :
    viewer->registerKeyboardCallback(&keyboardEventOccurred, (void *)NULL);
    drawClouds<PointNormalType>(viewer, cloudsIn, v1, true);
    viewer->initCameraParameters(); // 初始化摄像头参数
    // Set camera position and orientation
    // viewer->setCameraPosition(0, 0, 4, 0, 0, -1, v1);
    // viewer->setSize(1280, 1024); // Visualiser window size
    // 等待直到可视化窗口关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce();
        // The user pressed "space" :
        if (nextIteration)
        { // 循环处理所有点云
            size_t n = cloudsIn.size() - 1;
            for (size_t i = 0; i < n; i++)
            {
                auto &source = cloudsIn[i + 1]; // 连续配准
                auto &target = cloudsIn[i];     // 相邻两组点云
                // ------- modify this line for different register algorithm -----------
                PairAlign<PointNormalType>(*reg, source, target, pairTfs[i], maxIt);
                // ------- modify this line for different register algorithm -----------
            }
            Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
            for (size_t i = 1; i < cloudsIn.size(); i++)
            {
                tf = tf * pairTfs[i - 1];
                cloudsRes[i] = transformPointCloudWithNormal<PointNormalType>(cloudsIn[i], tf);
            }
            drawClouds<PointNormalType>(viewer, cloudsRes, v2, true);
            nextIteration = false;
        }
    }
    return (0);
}